import math
import numpy as np
import matplotlib.pyplot as plt

class PositionConvert(object):
    CONSTANTS_RADIUS_OF_EARTH = 6371000.     # meters (m)


    def GPStoXY(self, lat, lon, ref_lat, ref_lon):
        # input GPS and Reference GPS in degrees
        # output XY in meters (m) X:North Y:East
        lat_rad = math.radians(lat)
        lon_rad = math.radians(lon)
        ref_lat_rad = math.radians(ref_lat)
        ref_lon_rad = math.radians(ref_lon)

        sin_lat = math.sin(lat_rad)
        cos_lat = math.cos(lat_rad)
        ref_sin_lat = math.sin(ref_lat_rad)
        ref_cos_lat = math.cos(ref_lat_rad)

        cos_d_lon = math.cos(lon_rad - ref_lon_rad)

        arg = np.clip(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon, -1.0, 1.0)
        c = math.acos(arg)

        k = 1.0
        if abs(c) > 0:
            k = (c / math.sin(c))

        x = float(k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) * self.CONSTANTS_RADIUS_OF_EARTH)
        y = float(k * cos_lat * math.sin(lon_rad - ref_lon_rad) * self.CONSTANTS_RADIUS_OF_EARTH)

        return x, y

    def XYtoGPS(self, x, y, ref_lat, ref_lon):
        # input XY in meters (m) X:North Y:East and Reference GPS in degrees
        # output GPS in degrees
        x_rad = float(x) / self.CONSTANTS_RADIUS_OF_EARTH
        y_rad = float(y) / self.CONSTANTS_RADIUS_OF_EARTH
        c = math.sqrt(x_rad * x_rad + y_rad * y_rad)

        ref_lat_rad = math.radians(ref_lat)
        ref_lon_rad = math.radians(ref_lon)

        ref_sin_lat = math.sin(ref_lat_rad)
        ref_cos_lat = math.cos(ref_lat_rad)

        if abs(c) > 0:
            sin_c = math.sin(c)
            cos_c = math.cos(c)

            lat_rad = math.asin(cos_c * ref_sin_lat + (x_rad * sin_c * ref_cos_lat) / c)
            lon_rad = (ref_lon_rad + math.atan2(y_rad * sin_c, c * ref_cos_lat * cos_c - x_rad * ref_sin_lat * sin_c))

            lat = math.degrees(lat_rad)
            lon = math.degrees(lon_rad)

        else:
            lat = ref_lat
            lon = ref_lon

        return lat, lon

# (29.3585425, 106.0097503),1
if __name__ == '__main__':
    PC = PositionConvert()
    # 使用 PX4 SITL 默认参考点
    # <arg name="ref_lat" default="29.3585425"/>
    # <arg name="ref_lon" default="106.0097503"/>
    ref_lat = 29.3587363# 参考纬度
    ref_lon = 106.0087119# 参考经度
    # 参考纬度,参考经度lat, lon = 29.360886638348816, 106.01237347059129  # 测试经纬度跑道近端点(29.360886638348816, 106.01237347059129),1
        # (29.3585425, 106.0097503),1
        # (29.3585889, 106.0096288),2
        # (29.3586272, 106.0097972),3
        # (29.3586741, 106.0096709),4
        # (29.3587112, 106.0098365),5
        # (29.3587554, 106.0097068),6
        # (29.3587951, 106.0098752),7
        # (29.3588417, 106.0097434),8
        # (29.35271043548,106.00851582)机场端点1
    # qi1
    # latitude: 29.3588344
    # longitude: 106.0084597
    # altitude: 295.5424170648
    # (29.3587363, 106.0087119), qi2
    # (29.3586639, 106.0089146),qi3
    # (29.3586052, 106.0091428)qi4
    # # 测试点经纬度列表
    test_points = [
    # (29.3588344, 106.0084597),
    # (29.358571, 106.0092227), 
    # (29.3585084, 106.0093622),  
    # (29.3585425, 106.0097503),  
    # (29.3585889, 106.0096288),  
    # (29.3587951, 106.0098752),
    # (29.3588417, 106.0097434)
    (29.358522,106.009745)
    ]


    xy_points = []
    errors = []

    print("\n=== 坐标转换测试 ===")
    print(f"参考点经纬度: ({ref_lat:.6f}°, {ref_lon:.6f}°)")

    for i, (lat, lon) in enumerate(test_points):
        print(f"\n测试点 {i+1}:")
        print(f"原始经纬度: ({lat:.8f}°, {lon:.8f}°)")
        
        # 转换为XY坐标
        x, y = PC.GPStoXY(lat, lon, ref_lat, ref_lon)
        xy_points.append((x, y))
        print(f"转换后XY坐标: 北向{x:.8f}米, 东向{y:.8f}米")
        
        # 转换回经纬度以验证精度
        lat_back, lon_back = PC.XYtoGPS(x, y, ref_lat, ref_lon)
        print(f"转回经纬度: ({lat_back:.8f}°, {lon_back:.8f}°)")
        
        # 计算误差
        lat_error = abs(lat - lat_back)
        lon_error = abs(lon - lon_back)
        errors.append((lat_error, lon_error))
        print(f"转换误差: 纬度误差 {lat_error:.8f}°, 经度误差 {lon_error:.8f}°")

    # 分离XY坐标用于绘图
    x_coords = [point[0] for point in xy_points]
    y_coords = [point[1] for point in xy_points]

    # 创建图形和坐标轴
    fig, ax = plt.subplots(figsize=(10, 8))

    # 绘制散点图
    scatter = ax.scatter(x_coords, y_coords, s=100, c='blue', marker='o', alpha=0.7, edgecolors='black')

    # 添加参考点（用红色标记）
    ax.scatter(0, 0, s=150, c='red', marker='*', edgecolors='black', label='参考点')

    # 添加点的编号和坐标
    for i, (x, y) in enumerate(xy_points):
        ax.annotate(f'点{i+1}', (x, y), xytext=(5, 5), 
                    textcoords='offset points', fontsize=12)
        ax.annotate(f'({x:.2f}, {y:.2f})', (x, y), xytext=(0, -20),
                    textcoords='offset points', fontsize=10)

    # 设置图表标题和坐标轴标签
    ax.set_title('经纬度转换后的XY坐标散点图', fontsize=16)
    ax.set_xlabel('北向距离 (米)', fontsize=14)
    ax.set_ylabel('东向距离 (米)', fontsize=14)

    # 设置坐标轴刻度标签的大小
    ax.tick_params(axis='both', which='major', labelsize=12)

    # 添加网格线
    ax.grid(True, linestyle='--', alpha=0.7)

    # 添加图例
    ax.legend(loc='upper right', fontsize=12)

    # 添加误差信息文本框
    error_text = "坐标转换误差:\n"
    for i, (lat_error, lon_error) in enumerate(errors):
        error_text += f"点{i+1}: 纬度误差 {lat_error:.8f}°, 经度误差 {lon_error:.8f}°\n"

    ax.text(0.02, 0.98, error_text, transform=ax.transAxes,
            verticalalignment='top', bbox=dict(boxstyle='round', facecolor='wheat', alpha=0.5))

    # 调整布局
    plt.tight_layout()

    # 显示图形
    plt.show()

    
    # 测试1：小距离转换
    # print("\n测试1: 小距离XY转经纬度再转回XY")
    # x, y = -20.0, -20  # 北向x米，东向y米
    # print(f"原始XY坐标: 北向{x}米, 东向{y}米")
    # lat_new, lon_new = PC.XYtoGPS(x, y, ref_lat, ref_lon)
    # print(f"转换后经纬度: ({lat_new:.8f}°, {lon_new:.8f}°)")
    # x_back, y_back = PC.GPStoXY(lat_new, lon_new, ref_lat, ref_lon)
    # print(f"转回XY坐标: 北向{x_back:.8f}米, 东向{y_back:.8f}米")
    # print(f"转换误差: {abs(x-x_back):.8f}米, {abs(y-y_back):.8f}米")
    
    # # 测试2：大距离转换（北向5000米，东向3000米）
    # print("\n测试2: 大距离XY转经纬度")
    # x, y = 5000.0, 3000.0  # 北向5km，东向3km
    # print(f"原始XY坐标: 北向{x}米, 东向{y}米")
    # lat_new, lon_new = PC.XYtoGPS(x, y, ref_lat, ref_lon)
    # print(f"转换后经纬度: ({lat_new:.8f}°, {lon_new:.8f}°)")
    # x_back, y_back = PC.GPStoXY(lat_new, lon_new, ref_lat, ref_lon)
    # print(f"转回XY坐标: 北向{x_back:.8f}米, 东向{y_back:.8f}米")
    # print(f"转换误差: {abs(x-x_back):.8f}米, {abs(y-y_back):.8f}米")
     # 测试3：经纬度转换为XY再转回
    # print("\n测试3: 经纬度转XY再转回经纬度")
    # lat, lon = 29.3587834, 106.0085109 # 测试经纬度跑道近端点(29.360886638348816, 106.01237347059129),1
    # print(f"原始经纬度: ({lat:.8f}°, {lon:.8f}°)")
    # x, y = PC.GPStoXY(lat, lon, ref_lat, ref_lon)
    # print(f"转换后XY坐标: 北向{x:.8f}米, 东向{y:.8f}米")
    # lat_back, lon_back = PC.XYtoGPS(x, y, ref_lat, ref_lon)
    # print(f"转回经纬度: ({lat_back:.8f}°, {lon_back:.8f}°)")
    # print(f"转换误差: 纬度误差 {abs(lat-lat_back):.8f}°, 经度误差 {abs(lon-lon_back):.8f}°")
    #     # 测试3：经纬度转换为XY再转回
    # print("\n测试3: 经纬度转XY再转回经纬度")
    # lat, lon = 29.358672, 106.0088177 # 测试经纬度跑道近端点(29.360886638348816, 106.01237347059129),1
    # print(f"原始经纬度: ({lat:.8f}°, {lon:.8f}°)")
    # x, y = PC.GPStoXY(lat, lon, ref_lat, ref_lon)
    # print(f"转换后XY坐标: 北向{x:.8f}米, 东向{y:.8f}米")
    # lat_back, lon_back = PC.XYtoGPS(x, y, ref_lat, ref_lon)
    # print(f"转回经纬度: ({lat_back:.8f}°, {lon_back:.8f}°)")
    # print(f"转换误差: 纬度误差 {abs(lat-lat_back):.8f}°, 经度误差 {abs(lon-lon_back):.8f}°")
    #     # 测试3：经纬度转换为XY再转回
    # print("\n测试3: 经纬度转XY再转回经纬度")
    # lat, lon = 29.3586161, 106.008978 # 测试经纬度跑道近端点(29.360886638348816, 106.01237347059129),1
    # print(f"原始经纬度: ({lat:.8f}°, {lon:.8f}°)")
    # x, y = PC.GPStoXY(lat, lon, ref_lat, ref_lon)
    # print(f"转换后XY坐标: 北向{x:.8f}米, 东向{y:.8f}米")
    # lat_back, lon_back = PC.XYtoGPS(x, y, ref_lat, ref_lon)
    # print(f"转回经纬度: ({lat_back:.8f}°, {lon_back:.8f}°)")
    # print(f"转换误差: 纬度误差 {abs(lat-lat_back):.8f}°, 经度误差 {abs(lon-lon_back):.8f}°")
    # # 测试3：经纬度转换为XY再转回
    # print("\n测试3: 经纬度转XY再转回经纬度")
    # lat, lon = 29.3587268, 106.0086659 # 测试经纬度跑道近端点(29.360886638348816, 106.01237347059129),1
    # print(f"原始经纬度: ({lat:.8f}°, {lon:.8f}°)")
    # x, y = PC.GPStoXY(lat, lon, ref_lat, ref_lon)
    # print(f"转换后XY坐标: 北向{x:.8f}米, 东向{y:.8f}米")
    # lat_back, lon_back = PC.XYtoGPS(x, y, ref_lat, ref_lon)
    # print(f"转回经纬度: ({lat_back:.8f}°, {lon_back:.8f}°)")
    # print(f"转换误差: 纬度误差 {abs(lat-lat_back):.8f}°, 经度误差 {abs(lon-lon_back):.8f}°")
    
# [{"uav_id":1,"position":{"lon":106.0085109,"lat":29.3587834,"alt":70}},{"uav_id":2,"position":{"lon":106.0086659,"lat":29.3587268,"alt":70}},
# {"uav_id":3,"position":{"lon":106.0088177,"lat":29.358672,"alt":70}},{"uav_id":4,"position":{"lon":106.008978,"lat":29.3586161,"alt":70}}],
# "goals":[{"uav_id":1,"position":{"lon":106.0083301,"lat":29.3546487,"alt":70}},
# {"uav_id":2,"position":{"lon":106.0085032,"lat":29.3545869,"alt":70}},{"uav_id":3,"position":{"lon":106.0087121,"lat":29.3545128,"alt":70}},{"uav_id":4,"position":{"lon":106.0089128,"lat":29.3544451,"alt":70}}]}]



    # 矩形[0,0,0], [150,0,0], [150,100,0], [0,100,0]
    # x, y = 150, 0 
    # print(f"原始XY坐标: 北向{x}米, 东向{y}米")
    # lat_new, lon_new = PC.XYtoGPS(x, y, ref_lat, ref_lon)
    # print(f"转换后经纬度: ({lat_new:.8f}°, {lon_new:.8f}°)")

    # x, y = 150, 100  
    # print(f"原始XY坐标: 北向{x}米, 东向{y}米")
    # lat_new, lon_new = PC.XYtoGPS(x, y, ref_lat, ref_lon)
    # print(f"转换后经纬度: ({lat_new:.8f}°, {lon_new:.8f}°)")

    # x, y = 0, 100  
    # print(f"原始XY坐标: 北向{x}米, 东向{y}米")
    # lat_new, lon_new = PC.XYtoGPS(x, y, ref_lat, ref_lon)
    # print(f"转换后经纬度: ({lat_new:.8f}°, {lon_new:.8f}°)")